//
// Created by nvidia on 2021/4/28.
//

#ifndef ALLIANCE_SENTRY_V2_0_ADDITIONS_H
#define ALLIANCE_SENTRY_V2_0_ADDITIONS_H

#include <cstring>
#include <iostream>
//#include <fstream>
#include <opencv2/core.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/videoio/videoio_c.h>
#include <sys/stat.h>
#include <sys/types.h>

#include "options.h"
#include "systime.h"
#include <Eigen/Dense>

//创建文件夹
int makeDir(const std::string &filename_prefix,char *&p_cMkdir);
//视频保存函数
void saveVideos(const cv::Mat &src);
//显示原始图像
void showOrigin(const cv::Mat &src);
void showFps(cv::Mat &src);
//裁剪视频图像
bool videoExtract(cv::Mat &src);
//裁剪相机图像
bool cameraExtract(cv::Mat &src);
cv::VideoWriter initVideoWriter(const std::string &filename_prefix);
double getPointLength(const cv::Point2f &p);
double getPointLength(const cv::Point3f &p);
double getPointLength(const cv::Point2f &p1,const cv::Point2f &p2);
double getPointLength(const cv::Point3f &p1,const cv::Point3f &p2);
cv::Point2f angleToPixel(cv::Point2f pyangle);
cv::Point2f pixelToAngle(cv::Point2f point);
void calcRotMatrix(Eigen::Matrix<float,3,3> &R_vec,Eigen::Matrix<float,3,3> &R_back,const cv::Point2f pyangle);
cv::Point3f worldCoordinateResolver(cv::Point3f refer_coor, Eigen::Matrix<float,3,3> R_vec,float chassis_location);
cv::Point3f antiWorldCoordinateResolver(cv::Point3f world_coor,Eigen::Matrix<float,3,3> R_back,float chassis_location);
cv::Point2f referCoordinate2Angle(const cv::Point3f &refer_coor);
float time2frame(systime internal);

// 循环队列
template<class type, int _volume>
class RoundQueue {
private:
    type data[_volume];
    int head;//[0,volumn)
    int tail;//[0,volumn)


public:
    int volume;
    RoundQueue<type, _volume>() : head(0), tail(0),volume(_volume) {};
    ~RoundQueue()= default;

    void init(const type _init){
        for(int i=0;i<_volume;i++) data[i]=_init;
    }

    constexpr int size() const {
        return _volume;
    };

    bool empty() const {
        return head == tail;
    };

    void push(const type &obj) {
        data[head] = obj;
        head = (head + 1) % _volume;
        if (head == tail) {
            tail = (tail + 1) % _volume;
        }
    };

    type pop() {
        if (empty()) return false;
        type &obj = data[tail];
        tail = (tail + 1) % _volume;
        return obj;
    };

    type &operator[](int idx) {
        while (tail + idx < 0) idx += _volume;
        return data[(tail + idx) % _volume];
    };

    //geshu
    int length() const {
        if(head>=tail) return head-tail;
        else return _volume+1-(tail-head);

    }

    void clear(){
        head=0;
        tail=0;
    }

    double mean(){
        int len=length();
        type plus = 0;
        for(int i = 0;i<len;i++){
            plus+=(*this)[tail+i];
        }
        return (double)plus/(double)len;
    }

    double absMean(){
        int len=length();
        type plus = 0;
        for(int i = 0;i<len;i++){
            plus+=abs((*this)[tail+i]);
        }
        return (double)plus/(double)len;
    }

    double diffMean(){
        int len=length();
        type plus = 0;
        for(int i = 1;i<len;i++){
            plus+=(*this)[tail+i]-(*this)[tail+i-1];
        }
        return (double)plus/(double)(len-1);
    }

    double absDiffMean(){
        int len=length();
        type plus = 0;
        for(int i = 1;i<len;i++){
            plus+=abs((*this)[tail+i]-(*this)[tail+i-1]);
        }
        return (double)plus/(double)(len-1);
    }
};
#endif //ALLIANCE_SENTRY_V2_0_ADDITIONS_H
